#include "artcar_bringup.h"

long long LefttickPerMeter =0;    //左轮编码器每米脉冲数
long long righttickPerMeter =0;    //右轮编码器每米脉冲数
   /** 
    * @brief   小车类构造函数
    * @param  ros参数句柄，获取launch中的参数接口，获取失败则使用默认值
   	* @return    无返回值
    */
artcar::artcar(ros::NodeHandle nh)
{

    m_baudrate =  115200;               //波特率
    m_serialport = "/dev/ttyUSB0" ;          //串口号

    nh.param("mcubaudrate",m_baudrate,115200);                              //优先获取launch中的波特率
    nh.param("mcuserialport",m_serialport,string("/dev/ttyUSB0"));      //优先获取launch中定义传输的串口
	nh.param("calibrate",calibrate,calibrate);         //标定
    nh.param("ticksPerMeter",ticksPerMeter,ticksPerMeter);                           //一米脉冲数
    nh.param("ticksPer2PI",ticksPer2PI,ticksPer2PI);                                         //每圈脉冲数
    try{ //异常处理，未打开串口会报以下错误
      std::cout<<"[dzactuator-->]"<<"Serial initialize start!"<<std::endl;              
      ser.setPort(m_serialport.c_str());                                               
      ser.setBaudrate(m_baudrate);                                                    
      serial::Timeout to = serial::Timeout::simpleTimeout(30);                      
      ser.setTimeout(to);                                                            
      ser.open();                                                                     
    }
    catch (serial::IOException& e){
      std::cout<<"[dzactuator-->]"<<"Unable to open port!"<<std::endl;                
    }
    if (ser.isOpen())
    {
       std::cout<<"[dzactuator-->]"<<"Serial initialize successfully!"<<std::endl;       //如果串口打开，打印串口初始化成功
    }
     else{
      std::cout<<"[dzactuator-->]"<<"Serial port failed!"<<std::endl;                  
    } 
	sub_move_base = nh.subscribe("cmd_vel",1,&artcar::callback_move_base,this);  //订阅cmd_vel，实现move_base速度解算
    pub_imu = nh.advertise<sensor_msgs::Imu>("raw",5);                  //创建imu话题发布者
	pub_mag = nh.advertise<sensor_msgs::MagneticField>("imu/mag",5);			//创建磁力计话题发布者
	pub_odom = nh.advertise<nav_msgs::Odometry>("odom", 5);                                           //odom里程计数据发布
}


   /** 
    * @brief   小车运行函数的析构函数
    * @param      无参数
   	* @return    无返回值
    */
artcar::~artcar()
{

}
void artcar::callback_move_base(const geometry_msgs::Twist::ConstPtr &msg)
{
	//线速度，角速度转存
	linear_speed = msg->linear.x;
	angular_speed = msg->angular.z;
	sendCarInfoKernel();		//发送数据到STM32单片机
}
   /** 
    * @brief   小车运行函数
    * @param      无参数
   	* @return    无返回值
    */
void artcar::run()
{

	int run_rate = 50;			//定义ros的频率是50Hz
	ros::Rate rate(run_rate);
	ros::Time current_time,last_time;
	double x=0.0;       //x坐标 
   double y=0.0;        //y坐标
   double th = 0.0;     //定义旋转角度


	while (ros::ok)
	{
		ros::spinOnce();
		current_time = ros::Time::now();
		velDeltaTime = (current_time - last_time).toSec();          //转换成秒
		last_time = ros::Time::now();                     //当前时刻存放为上一时刻
		recvCarInforKernel();					//接收来自stm32的数据
		pub_9250();										//通过imu句柄发布话题数据
		 if (calibrate== 1)
    {
      printf("x=%.2f,y=%.2f,th=%.2f,linearSpeed=%.2f,angularSpeed=%.2f,detEncode=%.2f,LeftticksPerMeter = %lld,rightticksPerMeter = %lld\r\n",x,y,th,linearSpeed,angularSpeed,detEncode,LefttickPerMeter,righttickPerMeter);
    }

       finalencoderleft= (encoder1+encoder3)/2;               //计算左侧编码器平均值
    finalencoderright= (encoder2+encoder4)/2;          //计算右侧编码器平均值
        
    LefttickPerMeter = finalencoderleft+LefttickPerMeter;               //标定变量
    righttickPerMeter = finalencoderright+righttickPerMeter;       //标定变量
        
        detEncode=(finalencoderright+finalencoderleft)/2;                       //编码器平均值
    detdistance = detEncode/ticksPerMeter;                                                   //线速度标定距离变化
    detth = (finalencoderright-finalencoderleft)*2*PI/ticksPer2PI;             //角速度标定计算角度变化

    linearSpeed = detdistance/velDeltaTime;                           //运动学正解得到线速度
    angularSpeed = detth/velDeltaTime;                                   //运动学正解得到角速度
    

     if(detdistance != 0){
	        x += detdistance * cos(th);                        //x坐标
	        y += detdistance * sin(th);                        //y坐标
	    }
	    if(detth != 0){
	        th += detth;                                       //总角度
	    }
//   printf("%d,%d,%f,%f,%f,%f,%f,%f\n",finalencoderleft,finalencoderright,detEncode,detdistance,detth,x,y,th);

      geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);                      //odom角度变化
	    nav_msgs::Odometry odom;                               //创建nav_msgs::Odometry类型的消息odom
	    odom.header.stamp = current_time;                     
	    odom.header.frame_id = "odom";                         
 	    odom.child_frame_id = "base_link";                     

	    //set the position
	    odom.pose.pose.position.x = x;                       
	    odom.pose.pose.position.y = y;                        
	    odom.pose.pose.position.z = 0.0;                       
	    odom.pose.pose.orientation = odom_quat;               
	
	    odom.twist.twist.linear.x = linearSpeed;               //线速度
	    odom.twist.twist.linear.y = 0;
	    odom.twist.twist.linear.z = 0;
	    odom.twist.twist.angular.x = 0;
	    odom.twist.twist.angular.y = 0;
	    odom.twist.twist.angular.z = angularSpeed;             //角速度
	    if(finalencoderleft == 0 && finalencoderright == 0){
	        odom.pose.covariance = {1e-9, 0, 0, 0, 0, 0,       
				        0, 1e-3, 1e-9, 0, 0, 0,
				        0, 0, 1e6, 0, 0, 0,
				        0, 0, 0, 1e6, 0, 0,
				        0, 0, 0, 0, 1e6, 0,
				        0, 0, 0, 0, 0, 1e-9};
			odom.twist.covariance = {1e-9, 0, 0, 0, 0, 0,      
						0, 1e-3, 1e-9, 0, 0, 0,
						0, 0, 1e6, 0, 0, 0,
						0, 0, 0, 1e6, 0, 0,
						0, 0, 0, 0, 1e6, 0,
						0, 0, 0, 0, 0, 1e-9};
		}
		else{
			odom.pose.covariance = {1e-3, 0, 0, 0, 0, 0,       
						0, 1e-3, 0, 0, 0, 0,
						0, 0, 1e6, 0, 0, 0,
						0, 0, 0, 1e6, 0, 0,
						0, 0, 0, 0, 1e6, 0,
						0, 0, 0, 0, 0, 1e3};
			odom.twist.covariance = {1e-3, 0, 0, 0, 0, 0,      
						0, 1e-3, 0, 0, 0, 0,
						0, 0, 1e6, 0, 0, 0,
						0, 0, 0, 1e6, 0, 0,
						0, 0, 0, 0, 1e6, 0,
						0, 0, 0, 0, 0, 1e3};
		}
		pub_odom.publish(odom);       //发布里程计 

		rate.sleep();
	}

}

 /** 
    * @brief   小车发送数据到STM32的函数
    * @param      无参数
   	* @return    无返回值
    */
//发送小车数据到stm32下位机
void artcar:: sendCarInfoKernel()
{
	short transition;			//发送数据格式的中间变量
	SendBufer[0] = 0xa5;	//帧头
	SendBufer[1] = 0x5a;
	SendBufer[2] = 0x05;	//有效数据长度
	//发送机器人目标的线速度
	transition = 0;
	transition = linear_speed * 1000;		//将浮点数扩大1000倍，简化传输
	SendBufer[3] = transition >> 8;			//取数据的高8位
	SendBufer[4] = transition;			//取数据的低8位

	//发送机器人目标的角速度
	transition = 0;
	transition = angular_speed * 1000;	//将浮点数扩大1000倍，简化传输
	SendBufer[5] = transition>>8;	//取数据的高8位
	SendBufer[6] = transition;			//取数据的低8位
	//结尾使用异或校验(未编写)
	SendBufer[7] = 0x0F;
	 try
  {
    ser.write(SendBufer,sizeof (SendBufer)); //通过串口向下位机发送数据 
  }
  catch (serial::IOException& e)   
  {
    ROS_ERROR_STREAM("Unable to send data through serial port"); //如果发送数据失败，打印错误信息
  }
}
 /** 
    * @brief   小车接收STM32数据上传的函数
    * @param      无参数
   	* @return    无返回值
    */
//接收下位机发送来的数据
void artcar::recvCarInforKernel()
{    
    std::string recvstr;                                             
    unsigned char tempdata,lenrecv;                                  //定义字头,数据长度
    unsigned char count,last_data,last_last_data,last_last_last_data;  //定义3个变量轮询查找标志数据帧
    unsigned char str[100];                                          //定义数据缓冲区
    bool recvflag = false;                                          //数据完成接收标志，该标志为找到数据帧
    bool recvd_flag = false;                                         //数据完成接收标志,该标志为接收完一帧数据
    memset(&str,0,sizeof(str));                                      
    ros::Time begin_time = ros::Time::now();                         
    double clustering_time = 0;       

    //以下为查找数据帧，找到后跳出break循环，采用堵塞方式寻找，避免采集无效数据                               

    while(1){
	    clustering_time = (ros::Time::now () - begin_time).toSec (); //计算时间差，转换成秒
	    if(clustering_time > 1){                                    
	        recvd_flag = false;                                      
	        break;                                                   
	    }
		
	    recvstr = ser.read(1);                                      		
	    if((int)recvstr.size() != 1)                                 
	        continue;                                               
		
	    tempdata = recvstr[0];                                       
	    if(last_last_last_data == 0xa5 && last_last_data == 0x5a){   
	        lenrecv = last_data;                                     
	        recvflag = true;                                         
	        count = 0;                                               
	    }
	    if(recvflag){                                               
	        str[count] = tempdata;                                   
	        count++;                                                
	        if(count == lenrecv){                                   
		        recvflag = false;                                    
		        recvd_flag = true;                                   
		    break;                                                  
	        }
	    }
	    last_last_last_data = last_last_data;                        
	    last_last_data = last_data;                                  
	    last_data = tempdata;                                        
    }

    if(recvd_flag){                                                  //数据解析，接收到的数据转存
	    memcpy(&encoder1,str,4);                                  //电机1编码器增量值转存
	    memcpy(&encoder2,str+4,4);                               //电机2编码器增量值转存
		memcpy(&encoder3,str+8,4);								//电机3编码器增量值转存
		memcpy(&encoder4,str+12,4);							//电机4编码器增量值转存
	    memcpy(&batteryVoltage,str+16,4);                             //电池电压值转存
 	
	    memcpy(&tempaccelX,str+20,2);                                //X线加速度转存
	    memcpy(&tempaccelY,str+22,2);                                //Y线加速度转存
		memcpy(&tempaccelZ,str+24,2);                                //z线加速度转存
		
		memcpy(&tempgyroX,str+26,2);                                 //X轴角速度转存
		memcpy(&tempgyroY,str+28,2);                                 //Y轴角速度转存
		memcpy(&tempgyroZ,str+30,2);                                 //z轴角速度转存
		
		memcpy(&tempmagX,str+32,2);                                  //X轴磁力计转存
		memcpy(&tempmagY,str+34,2);                                  //Y轴磁力计转存
		memcpy(&tempmagZ,str+36,2);                                  //Z轴磁力计转存
	
		memcpy(&distanceA,str+38,4);                                 //超声距离值A转存
		memcpy(&distanceB,str+42,4);                                 //超声距离值B转存

		accelX = (float)tempaccelX/2048*9.8;                         //线加速度处理	
		accelY = (float)tempaccelY/2048*9.8;
		accelZ = (float)tempaccelZ/2048*9.8;

		
		gyroX = (float)tempgyroX/16.4/57.3;                          //角速度处理
		gyroY = (float)tempgyroY/16.4/57.3;
		gyroZ = (float)tempgyroZ/16.4/57.3;
		
		magX = (float)tempmagX*0.14;                                 //磁力计处理
		magY = (float)tempmagY*0.14;
		magZ = (float)tempmagZ*0.14;
    }
}
void artcar::pub_9250()
{
	sensor_msgs::Imu imuMsg;				//定义imu数据结构体
	sensor_msgs::MagneticField magMsg;			//定义磁力计数据结构体
	ros::Time current_time = ros::Time::now();				
	imuMsg.header.stamp = current_time;
	imuMsg.header.frame_id = "imu_link";

	imuMsg.linear_acceleration.x = accelX;				//x方向线加速度
	imuMsg.linear_acceleration.y = accelY;				//y方向线加速度
	imuMsg.linear_acceleration.z = accelZ;				//z方向线加速度
	//IMU线加速度方向的协方差矩阵
	imuMsg.linear_acceleration_covariance = {
		0.04,0.0,0.0,
		0.0,0.04,0.0,
		0.0,0.0,0.04
	};
	imuMsg.angular_velocity.x = gyroX;				//x方向的角速度
	imuMsg.angular_velocity.y = gyroY;				//y方向的角速度
	imuMsg.angular_velocity.z = gyroZ;				//z方向的角速度
	//IMU角速度方向的协方差矩阵
	imuMsg.angular_velocity_covariance = {
		0.04,0.0,0.0,
		0.0,0.04,0.0,
		0.0,0.0,0.04
	};
	
	// Quaternion_Solution(imuMsg.angular_velocity.x,  \
	// 											 imuMsg.angular_velocity.y,   \
	// 											 imuMsg.angular_velocity.z,   \
	// 											 imuMsg.linear_acceleration.x,	\
	// 											 imuMsg.linear_acceleration.y,  \
	// 											 imuMsg.linear_acceleration.z,  \
	// 											 &imuMsg);

	// imuMsg.orientation_covariance[0] = 1e6; //三轴姿态协方差矩阵
  	// imuMsg.orientation_covariance[4] = 1e6;
  	// imuMsg.orientation_covariance[8] = 1e-6;  
    pub_imu.publish(imuMsg);					//发布数据

	magMsg.header.stamp = current_time;
	magMsg.header.frame_id = "base_link";
	magMsg.magnetic_field.x = magX;
	magMsg.magnetic_field.y = magY;
	magMsg.magnetic_field.z = magZ;
	magMsg.magnetic_field_covariance = {
		0.0,0.0,0.0,
		0.0,0.0,0.0,
		0.0,0.0,0.0
	};
	pub_mag.publish(magMsg);         //发布磁力计
	
}
